/*
 * RT MAP, Home of Professional MAP
 * Copyright 2021 Bit Main Inc. and/or its affiliates and other contributors
 * as indicated by the @author tags. All rights reserved.
 * See the copyright.txt in the distribution for a
 * full listing of individual contributors.
 */
package com.rw.map.utils;

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.PriorityQueue;

import com.rw.map.model.Path;
import com.rw.map.model.Point;
import com.rw.map.model.Vertex;

/**
 * @author CaoYuyao
 * @title NaviTool
 * @description 计算路径
 * @date 2021/9/10
 */
public class NaviTool {
	private ArrayList<Path> paths;
	private ArrayList<Point> points;
	private HashMap<String, List<Vertex>> vertices;
	
	/**
	 * 记录路线宽度，用于偏移
	 */
	private HashMap<String, Double> offsetMap;
	private HashMap<String, String> cameraMap;

	public HashMap<String, String> getCameraMap() {
		return cameraMap;
	}

	public NaviTool(ArrayList<Point> points, ArrayList<Path> paths) {
		this.paths = paths;
		this.points = points;
		this.vertices = this.initData(this.points, this.paths);
		this.offsetMap = this.initOffset(this.paths);
		this.cameraMap = this.initCamera(this.points);
	}

	private HashMap<String, String> initCamera(ArrayList<Point> points) {
		HashMap<String, String> cameraMap = new HashMap<String, String>();
		for (Point point : points) {
			if (point.getCamera() != null && !"".equals(point.getCamera())) {
				cameraMap.put(point.getCamera(), point.getId());
			}
		}

		return cameraMap;
	}

	/**
	 * 初始化offsetMap数据
	 *
	 * @param paths 路线数据
	 * @return offsetMap数据
	 */
	private HashMap<String, Double> initOffset(ArrayList<Path> paths) {
		HashMap<String, Double> offsetMap = new HashMap<>();
		for (Path path : paths) {
			offsetMap.put(path.getStart() + "_" + path.getEnd(), path.getWidth());
			offsetMap.put(path.getEnd() + "_" + path.getStart(), path.getWidth());
		}
		return offsetMap;
	}

	/**
	 * 路径规划
	 *
	 * @param cameraStart 起点摄像头
	 * @param cameraEnd   终点摄像头
	 * @return 结果
	 * @throws Exception
	 */
	public List<Point> navigateByCamera(String cameraStart, String cameraEnd) throws Exception {
		String start = this.cameraMap.get(cameraStart);
		if (start == null || "".equals(start)) {
			throw new Exception("未找到起点摄像头");
		}

		String end = this.cameraMap.get(cameraEnd);
		if (end == null || "".equals(end)) {
			throw new Exception("未找到终点摄像头");
		}

		return this.navigate(start, end);
	}

	/**
	 * 路径规划
	 *
	 * @param start 起点id
	 * @param end   终点id
	 * @return 结果
	 * @throws Exception
	 */
	public List<Point> navigate(String start, String end) throws Exception {
		if (this.paths == null || this.points == null || this.vertices == null || this.offsetMap == null) {
			throw new Exception("未初始化");
		}
		if (start.equals(end)) {
			throw new Exception("起点终点不能一样");
		}
		List<String> shortestPath = this.getShortestPath(start, end);
		if (shortestPath == null || shortestPath.size() < 2) {
			throw new Exception("路径规划失败");
		}
		List<Point> result = new ArrayList<>();
		double width = 0;
		for (int i = 0; i < shortestPath.size(); i++) {
			String id = shortestPath.get(i);
			for (Point point : this.points) {
				if (point.getId().equals(id)) {
					if (i == 0) {
						String idEnd = shortestPath.get(i + 1);
						width = this.getOffsetWidth(id, idEnd);

					} else if (i == shortestPath.size() - 1) {
						String idStart = shortestPath.get(i - 1);
						width = this.getOffsetWidth(idStart, id);

					} else {
						String idPrev = shortestPath.get(i - 1);
						String idNext = shortestPath.get(i + 1);
						double widthPrev = this.getOffsetWidth(idPrev, id);
						double widthNext = this.getOffsetWidth(id, idNext);
						width = Math.min(widthPrev, widthNext);
					}
					Point point1 = point.clone();
					point1.setRandomOffset(width);
					result.add(point1);
				}
			}
		}
		return result;
	}

	/**
	 * 处理数据，将点和线制作成路网
	 *
	 * @param points 点数据
	 * @param paths  线数据
	 * @return 路网数据
	 */
	public HashMap<String, List<Vertex>> initData(ArrayList<Point> points, ArrayList<Path> paths) {
		// 解析数据，确定点和周围的点的关系
		HashMap<String, List<Vertex>> vertexMap = new HashMap<String, List<Vertex>>();
		for (Point point : points) {
			// 计算和point相连的path
			List<Vertex> neighborList = new ArrayList<Vertex>();
			for (Path path : paths) {
				if (path.getStart().equals(point.getId())) {
					neighborList.add(new Vertex(path.getEnd(), path.getLength()));
				} else if (path.getEnd().equals(point.getId())) {
					neighborList.add(new Vertex(path.getStart(), path.getLength()));
				}
			}
			vertexMap.put(point.getId(), neighborList);
		}

		return vertexMap;
	}

	/**
	 * @param start 起点id
	 * @param end   终点id
	 * @return 路径id
	 */
	public List<String> getShortestPath(String start, String end) {
		// 所有节点和起点的距离
		final Map<String, Double> distances = new HashMap<String, Double>();
		// 上一个点
		final Map<String, Vertex> previous = new HashMap<String, Vertex>();
		// 所有待处理节点，使用距离排序
		PriorityQueue<Vertex> nodes = new PriorityQueue<Vertex>();

		// 找到起点，初始化nodes
		for (String id : this.vertices.keySet()) {
			if (id.equals(start)) {
				distances.put(id, 0d);
				nodes.add(new Vertex(id, 0));
			} else {
				distances.put(id, Double.MAX_VALUE);
				nodes.add(new Vertex(id, Double.MAX_VALUE));
			}
			previous.put(id, null);
		}

		while (!nodes.isEmpty()) {
			// 取出待处理的点中距离最短的
			Vertex smallest = nodes.poll();
			// 找到终点退出
			if (smallest.getId().equals(end)) {
				final List<String> path = new ArrayList<String>();
				while (previous.get(smallest.getId()) != null) {
					path.add(smallest.getId());
					smallest = previous.get(smallest.getId());
				}
				path.add(start);
				Collections.reverse(path);

				return path;
			}
			// 如果找不到新的最近点则代表无路可走，退出
			if (distances.get(smallest.getId()) == Double.MAX_VALUE) {
				break;
			}
			// 找到最近点的邻近点
			for (Vertex neighbor : vertices.get(smallest.getId())) {
				// 计算邻近点和起点的距离
				double neighborDistance = distances.get(smallest.getId()) + neighbor.getDistance();
				// 如果该距离比记录的距离短则记录
				if (neighborDistance < distances.get(neighbor.getId())) {
					distances.put(neighbor.getId(), neighborDistance);
					previous.put(neighbor.getId(), smallest);
					// 更新距离
					for (Vertex n : nodes) {
						if (n.getId().equals(neighbor.getId())) {
							nodes.remove(n);
							n.setDistance(neighborDistance);
							nodes.add(n);
							break;
						}
					}
				}
			}
		}
		return null;
	}

	private Double getOffsetWidth(String start, String end) throws Exception {
		if (this.offsetMap == null) {
			throw new Exception("未初始化");
		}
		String key = start + "_" + end;
		return this.offsetMap.getOrDefault(key, 0d);
	}

}
